#include "projection.h"

projection::projection()
{
	rotation(0,0,0);
}

void projection::rotation(float y,float p,float r)
{
	float TempMatrix[AFINE_DIM][AFINE_DIM];
	alpha=y;
	beta=p;
	gamma=r;
	yaw_M(y);
	pitch_M(p);
	roll_M(r);


//      Roll*Pitch*Yaw
        MatrixMult((float*)RollMatrix,(float*)PitchMatrix,AFINE_DIM,AFINE_DIM,AFINE_DIM,(float*)TempMatrix);
        MatrixMult((float*)TempMatrix,(float*)YawMatrix,AFINE_DIM,AFINE_DIM,AFINE_DIM,(float*)RotationMatrix);
}

//ROLL Z-axis rotation
void projection::yaw_M(float angle)
{
		float coseno,sine;
		coseno=cos(angle);
		sine=sin(angle);
		YawMatrix[0][0]=coseno;
		YawMatrix[0][1]=-sine;
		YawMatrix[0][2]=0;
		YawMatrix[1][0]=sine;
		YawMatrix[1][1]=coseno;
		YawMatrix[1][2]=0;
		YawMatrix[2][0]=0;
		YawMatrix[2][1]=0;
		YawMatrix[2][2]=1;
}

//ROLL Y-axis rotation
void projection::pitch_M(float angle)
{
		float coseno,sine;
		coseno=cos(angle);
		sine=sin(angle);
		PitchMatrix[0][0]=coseno;
		PitchMatrix[0][1]=0;
		PitchMatrix[0][2]=sine;
		PitchMatrix[1][0]=0;
		PitchMatrix[1][1]=1;
		PitchMatrix[1][2]=0;
		PitchMatrix[2][0]=-sine;
		PitchMatrix[2][1]=0;
		PitchMatrix[2][2]=coseno;
}

//ROLL X-axis rotation
void projection::roll_M(float angle)
{
		float coseno,sine;
		coseno=cos(angle);
		sine=sin(angle);
		RollMatrix[0][0]=1;
		RollMatrix[0][1]=0;
		RollMatrix[0][2]=0;
		RollMatrix[1][0]=0;
		RollMatrix[1][1]=coseno;
		RollMatrix[1][2]=-sine;
		RollMatrix[2][0]=0;
		RollMatrix[2][1]=sine;
		RollMatrix[2][2]=coseno;

}

#ifdef SERIALDEBUG
void projection::printRotation()
{
	Serial.println();
	Serial.print("ANGLES(y,p,r): ");
	Serial.print(alpha*180/M_PI);
	Serial.print(" ");
	Serial.print(beta*180/M_PI);
	Serial.print(" ");
	Serial.print(gamma*180/M_PI);
	Serial.println();
	Serial.println("Z AXIS:YAW MATRIX:");
	MatrixPrint((float *)YawMatrix,AFINE_DIM,AFINE_DIM);
	Serial.println();
	Serial.println("Y' AXIS:PITCH MATRIX:");
	MatrixPrint((float *)PitchMatrix,AFINE_DIM,AFINE_DIM);
	Serial.println();
	Serial.println("X'' AXIS:ROLL MATRIX:");
	MatrixPrint((float *)RollMatrix,AFINE_DIM,AFINE_DIM);
	Serial.println();
	Serial.println("ROTATION MATRIX:");
	MatrixPrint((float *)RotationMatrix,AFINE_DIM,AFINE_DIM);
	Serial.println();


}
#endif

void projection::project()
{
	float temp_vector[AFINE_DIM][1];
        MatrixMult((float*)RotationMatrix,(float*)vector_M,AFINE_DIM,AFINE_DIM,1,(float*)temp_vector);
        MatrixCopy((float*)temp_vector,AFINE_DIM,1,(float*)vector_M);
}


void projection::projectOrtho(float &x,float &y)
{
        project();
	x=vector_M[0][0];
	y=vector_M[1][0];	
   
}




void projection::point(float x,float y,float z)
{
	vector_M[0][0]=x;
	vector_M[1][0]=y;
	vector_M[2][0]=z;
}

/*
vector projection::rotationAngles()
{
	vector v;

	v.x=atan2(RotationMatrix[1][0],RotationMatrix[0][0]);
	v.y=atan2(RotationMatrix[2][1],RotationMatrix[2][2]);
	v.z=asin(RotationMatrix[2][0]);

	return v;
}
*/

